/*
 *  Copyright (C) 2004 Steve Harris, Uwe Koloska
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  $Id: example_client.c,v 1.1.1.1 2004/08/07 22:21:02 theno23 Exp $
 */

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>

#include "Vector3.h"
#include "lo/lo.h"

lo_address t;

Vector3 wincha;
Vector3 winchb;
Vector3 winchc;
Vector3 target_;

int amin = 26508;
int bmin = 27764;
int cmin = 30380;

int amax = 36719;
int bmax = 38348;
int cmax = 39665;

int aoffset = 3254;
int boffset = 3696;
int coffset = 926;

sendtotarget(Vector3 target){
  int a = (int)vecLength(vecSub(wincha, target));
  int b = (int)vecLength(vecSub(winchb, target));
  int c = (int)vecLength(vecSub(winchc, target));
  a = a - aoffset;
  b = b - boffset;
  c = c - coffset;
  a = amax - a;
  b = bmax - b;
  c = cmax - c;
  printf("A:%d B:%d C:%d\n",a,b,c);
  sendtoposition("/pfs/ur/0007/pb/1/axis/0002/setposition", 2, a);
  sendtoposition("/pfs/ur/0007/pb/2/axis/0005/setposition", 5, c);
  sendtoposition("/pfs/ur/0007/pb/3/axis/0007/setposition", 7, b);
}

sendtoposition(char* path, int axis, int position){
  lo_message message = lo_message_new();
  lo_message_add(message, "iii", 0, axis, position);
  lo_bundle bundle = lo_bundle_new( LO_TT_IMMEDIATE );
  lo_bundle_add_message(bundle, path, message); 
  lo_send_bundle(t, bundle);
  lo_bundle_free(bundle);
  lo_message_free(message);
}

startplayback(char* path){
  lo_message message = lo_message_new();
  lo_message_add(message, "i", 0);
  lo_bundle bundle = lo_bundle_new( LO_TT_IMMEDIATE );
  lo_bundle_add_message(bundle, path, message); 
  lo_send_bundle(t, bundle);
  lo_bundle_free(bundle);
  lo_message_free(message);
}

setspeed(char* path, int speed){
    lo_message message = lo_message_new();
    lo_message_add(message, "ii", 0, speed);
    lo_bundle bundle = lo_bundle_new( LO_TT_IMMEDIATE );
    lo_bundle_add_message(bundle, path, message); 
    lo_send_bundle(t, bundle);
    lo_bundle_free(bundle);
    lo_message_free(message);
}

int main(int argc, char *argv[])
{
  t = lo_address_new("192.168.2.57", "25666");

  target_.x = 2459;
  target_.y = 5847;
  target_.z = 5608;

	wincha.x = 0;
	wincha.y = 0;
	wincha.z = 0;

	winchb.x = 0;
	winchb.y = 0;
	winchb.z = 12192;

	winchc.x = 4877;
	winchc.y = 0;
	winchc.z = 6706;

  if(strcmp(argv[1],"0") == 0){
        setspeed("/pfs/ur/0006/pb/1/setSpeed", 50);
	//usleep(10000);
        setspeed("/pfs/ur/0006/pb/2/setSpeed", 50);
	//usleep(10000);
        setspeed("/pfs/ur/0006/pb/3/setSpeed", 50);
	//usleep(10000);
        setspeed("/pfs/ur/0006/pb/4/setSpeed", 50);
	//usleep(10000);
        sendtoposition("/pfs/ur/0006/pb/1/axis/0002/setposition", 2, 35000);
	//usleep(10000);
        sendtoposition("/pfs/ur/0006/pb/2/axis/0003/setposition", 3, 35000);
	//usleep(10000);
        sendtoposition("/pfs/ur/0006/pb/3/axis/0006/setposition", 6, 35000);
	//usleep(10000);
        sendtoposition("/pfs/ur/0006/pb/4/axis/0007/setposition", 7, 35000);

  }
  if(strcmp(argv[1],"5") == 0){
        setspeed("/pfs/ur/0006/pb/1/setSpeed", 50);
	usleep(10000);
        setspeed("/pfs/ur/0006/pb/2/setSpeed", 50);
	usleep(10000);
        setspeed("/pfs/ur/0006/pb/3/setSpeed", 50);
	usleep(10000);
        setspeed("/pfs/ur/0006/pb/4/setSpeed", 50);
	usleep(10000);
        sendtoposition("/pfs/ur/0006/pb/1/axis/0002/setposition", 2, 31050);
	usleep(10000);
        sendtoposition("/pfs/ur/0006/pb/2/axis/0003/setposition", 3, 31050);
	usleep(10000);
        sendtoposition("/pfs/ur/0006/pb/3/axis/0006/setposition", 6, 31050);
	usleep(10000);
        sendtoposition("/pfs/ur/0006/pb/4/axis/0007/setposition", 7, 31050);

  }
  if(strcmp(argv[1],"1") == 0){
	if(strcmp(argv[2], "4") == 0){
		setspeed("/pfs/ur/0007/pb/1/setSpeed", 0);
		printf("CRASHING\n");
	}
    if(strcmp(argv[2], "1") == 0){
      startplayback("/pfs/ur/0007/pb/1/start");
	printf("TEST\n");
    }
    if(strcmp(argv[2], "2") == 0){
      startplayback("/pfs/ur/0007/pb/2/start");
    }
    if(strcmp(argv[2], "3") == 0){
      startplayback("/pfs/ur/0007/pb/3/start");
    }
  }
  if(strcmp(argv[1],"7") == 0){
      sendtoposition("/pfs/ur/0007/pb/1/axis/0003/setposition", 3, 6000);
      sendtoposition("/pfs/ur/0007/pb/1/axis/0004/setposition", 4, 6000);
      sendtoposition("/pfs/ur/0007/pb/1/axis/0008/setposition", 8, 6000);
  }
  if(strcmp(argv[1],"3") == 0){
    sendtoposition("/pfs/ur/0007/pb/2/axis/0005/setposition", 5, 32500);
  }
  if(strcmp(argv[1],"2") == 0){
    lo_message message = lo_message_new();
    lo_message_add(message, "i", 13000);
    lo_bundle bundle = lo_bundle_new( LO_TT_IMMEDIATE );
    lo_bundle_add_message(bundle, "/pfs/ur/0007/nextState", message); 
    lo_send_bundle(t, bundle);
    lo_bundle_free(bundle);
    lo_message_free(message);
  }
  if(strcmp(argv[1],"4") == 0){
    sendtotarget(target_);
  }

  return 0;
}

